#ifndef __LOG_H__
#define __LOG_H__

#include <time.h>
#include <sys/time.h> 
#include <string.h>
#include <pthread.h>
#include <errno.h>
#include <stdio.h>
#include <unistd.h>
#include <semaphore.h>
#include <math.h>

#include "../vision/xy_vision.h"
#include "../pilot/autopilot_interface.h"
#include "../ultrasonic/ultrasonic.h"
#include "../commander/commander.h"
#include "../controller/controller_3D_posPID.h"
#include "../navigator/ref_navigator.h"
#include "../rotate_speed/speed_test.h"

//#include "../main.h"

#define CRLF "\n"

class xy_log_c
{
public:
		xy_log_c(autopilot_interface_c &_pilot, ultrasonic_c &_xy_ultra, xy_vision_c &_xy_vision,ref_navigator_c &_ref_navigator_current, controller_3D_posPID_c &_controller_current);
		~xy_log_c();
		
		int init_log(int *fd);
		int XY_SD_Log_Setup(void);
		void store_depend_stat(int _stat, char *strp);
		void check_stat_store(int _stat, int _mask, char *strp);
		void add_crlf(void);
		void set_log_on_flag(int _val);
		void clear_log_on_flag(void);
		int get_log_on_flag(void);
		int if_log_on(void);
		void wait_log_on(void);
		int XY_Start_Store_Log(void);
		int XY_Stop_Store_Log(void);
		void store_log_thread();
		
		sem_t log_ctrl_data_sem;
		sem_t log_start_sem;
		pthread_mutex_t log_ctrl_lock = PTHREAD_MUTEX_INITIALIZER;
		pthread_mutex_t log_on_off_lock = PTHREAD_MUTEX_INITIALIZER;
		int log_on_flag;
		
		autopilot_interface_c &pilot;
		ultrasonic_c &xy_ultra;
		xy_vision_c &xy_vision;
	    ref_navigator_c &ref_navigator_current;
		controller_3D_posPID_c &controller_current;
		
		int log_fd;
		char log_name[50];
		
		float log_height_ultra;
		mavlink_global_position_int_t log_GPS_position_part1;
		mavlink_gps_raw_int_t log_GPS_position_part2;
		mavlink_local_position_ned_t log_local_position;
		mavlink_attitude_t log_attitude;
		mavlink_heartbeat_t log_system_status_part1;
		mavlink_extended_sys_state_t log_system_status_part2;
		mavlink_sys_status_t log_system_status_part3;
		mavlink_servo_output_raw_t log_servo_output_raw;
		ref_point_t log_controller_ref_point;
		ref_point_t log_controller_ref_feedback;
		d3_control_t log_d3_control;
		ref_position_vision_t log_vision_position;
		float log_vision_height;
		ultra_data_t log_ultra_data;
		float log_filtered_ultra_data;
		int log_gpio21_rev_speed;
		int log_gpio18_rev_speed;
		int log_chan3_raw;
};

void* start_store_to_log_thread_func(void *args);
#endif